//////////////////////////////
// serial buffer loading
const int _hashcmd='*';
const int _startcmd='#';
const int _endcmd='$';

const int _iMaxSerialBuffer = 512;
const int _iMaxSerialHash = 8;
int _iSerialHashLen=0, _iSerialBufferLen=0;
char _SerialBuffer[_iMaxSerialBuffer];
char _SerialBufferHash[_iMaxSerialHash];
int _iReadMode = 0;

boolean readSerialString (String &cmd) {
  int sb;

  while (Serial.available())
  {
    sb = Serial.read();
    if(sb == _hashcmd) // header/hash ?
    {
      _iReadMode = 1;
      _iSerialHashLen = 0;
      continue;
    }
    else if(sb == _startcmd) // start cmd ?
    {
      _iReadMode=2;
      _SerialBufferHash[_iSerialHashLen] = '\0';
      _iSerialBufferLen=0;
      continue;
    }
    else if(sb==_endcmd) // end cmd ?
    {
      if(_iReadMode!=2)
      {
        _iReadMode=0; 
        continue;
      }
      
      _iReadMode=0;
      
      if(_iSerialHashLen>0)
      {
        // verify hash !
        String sCmdHash = _SerialBufferHash;
        byte iCmdHash = sCmdHash.toInt();
  
        byte iHash=0;
        for(int t=0; t<_iSerialBufferLen; t++) iHash += (byte)(_SerialBuffer[t]);
        if(iHash == iCmdHash)
        {
          _SerialBuffer[_iSerialBufferLen] = '\0';
          cmd = _SerialBuffer;
          return true;
        }
  
        send_error("bad hash !");
        return false;
      }
      else
      {
          _SerialBuffer[_iSerialBufferLen] = '\0';
          cmd = _SerialBuffer;
          return true;
      }
    }

    if(_iReadMode==1)
    {
      if(_iSerialHashLen>=_iMaxSerialHash)
      {
        _iReadMode = 0;
        send_error("too long hash !");
        continue;
      }

      _SerialBufferHash[_iSerialHashLen]=sb;
      _iSerialHashLen++;
    }
    else if(_iReadMode==2)
    {
      if(_iSerialBufferLen>=_iMaxSerialBuffer)
      {
        _iReadMode = 0;
        send_error("too long request !");
        continue;
      }

      _SerialBuffer[_iSerialBufferLen] = sb;
      _iSerialBufferLen++;
    }
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, const int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}
int load_int_nosigne(String &s, const int start, int &iRet)
{
  int iLen = start;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";

  for(int i=0; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

//////////////////////////////
// commands analysing
void check_commands(void)
{
  String line="";
  while(readSerialString(line))
  {
    if(line.length()==0) continue;
    {
      // debug only : 
      send_message(line);
  
      switch(line[0])
      {
      case 'v': // (V)ersion
        {
          // #v$
          send_version();
        }
        break;      
      case 'g': // settin(G)
        {
          switch(line[1])
          {
          case 'a': // (A)sk
            {
              switch(line[2])
              {
              case 'b':  // (B)ody setting
                // #gab$
                send_setting_body();        
                break;
              case 't':  // (T)urrel setting
                // #gat$
                send_setting_turret();
                break;
              case 'r':  // (R)ay setting
                // #gar$
                send_setting_ray();
                break;
              default: // general setting
                // #ga-$
                send_setting();
                break;
              }
            }
            break;
          case 's': // (S)et
            {
              switch(line[2])
              {
              case 'r':  // (R)ay setting
                {
                  switch(line[3])
                  {
                  case 'c':  // ray (C)ount setting
                    {
                      // #gsrc5$
                      int bc=_beam_count;
                      int lbs = load_int_nosigne(line, 4, bc);
                      _beam_count = bc;
                      send_setting_ray();
                    }
                    break;
                  case 'd':  // ray (D)elay setting
                    {
                      // #gsrd5$
                      int br=_beam_delay;
                      int lbs = load_int_nosigne(line, 4, br);
                      _beam_delay = br;
                      send_setting_ray();
                    }
                    break;
                  case 's':  // ray (S)ensors setting
                    {
                      // #gsrs3$
                      int bs=_beam_sensors;
                      int lbs = load_int_nosigne(line, 4, bs);
                      _beam_sensors = bs;
                      send_setting_ray();
                    }
                    break;
                  case 'p':  // ray (P)recision setting
                    {
                      // #gsrs3$
                      int bp=_beam_precision*100;
                      int lbs = load_int_nosigne(line, 4, bp);
                      _beam_precision = (float)bp/100.0f;
                      send_setting_ray();
                    }
                    break;
                  }
                }
                break;
              }
            }
            break;
          }
        } 
        break;            
      case 's': // (S)tatus    
        {
          switch(line[1])
          {
          case 'a': // (A)sk
            {
              switch(line[2])
              {
              case 's':  // (S)ensors
                // #sas$
                send_status_hitsensors();
                break;
              case 't':  // (T)urrel
                // #sat$
                send_status_turret();
                break;
              case 'b':  // (B)ody
                // #sab$
                send_status_body();
                break;
              }
            }
            break;
          case 's': // (S)et
            {
              switch(line[2])
              {
              case 's': // (S)end status automatic
                {
                  // #sss0$/#sss1$
                  char param = line[3];
                  if(param=='0') _send_status = false;
                  else if(param=='1') _send_status = true;
                  send_setting();
                }
                break;
              }
            }
            break;
          }
        }
        break;
      case 'r': // (R)eset
        {
          switch(line[1])
          {
          case 'b': // (B)ody
            // #rb$
            motor_stop();
            break;
          case 't': // (T) turret
            // #rt$
            reset_turret_position(100);
            if(!_send_status) send_status_turret();
            break;
          case 'g': // tar(G)et
            // #rg$
            target_stop();
            break;
          default: // All !
            // #r-$
            target_stop();
            motor_stop(); 
            reset_turret_position(100);
            if(!_send_status) send_status_turret();
            break;
          }
        }
        break;      
      case 'b': // (B)ody stuff
        {
          switch(line[1])
          {
          case 'm': // 'M'oving
            {
              // #bm11+25+25$
              boolean bL = (line[2]=='0')?false:true;
              boolean bR = (line[3]=='0')?false:true;
              int iL=0, iR=0;
              int endl = load_int(line, 4, iL);
              load_int(line, endl, iR);
              int iPwnL = map_power_motor(iL);
              int iPwnR = map_power_motor(iR);
              motor_action(iPwnL, iPwnR, bL, bR);
            }
            break;
          case 't': // 'T'argetting
            {
              switch(line[2])
              {
              case 'm': // 'M'oving
                {
                  // #btm1+25+15$
                  boolean bF = (line[3]=='0')?false:true;
                  int iP=0, iLen=0;
                  int endp = load_int(line, 4, iP);
                  load_int(line, endp, iLen);
                  move_to_target(bF, iP, iLen);
                }
                break;
              case 'r': // 'R'otating
                {
                  // #btr+25+250$
                  int iP=0, iBear=0;
                  int endp = load_int(line, 3, iP);
                  load_int(line, endp, iBear);
                  rotate_to_target(iP, iBear);
                }
                break;
              }
            }
            break;
          }
        }
        break;      
      case 't': // (T)urrel stuff
        {
          switch(line[1])
          {
          case 'p': // set 'P'osition
            {
              // #tp+90+90$
              int iLow=0, iHigh=0;
              int endl = load_int(line, 2, iLow);
              load_int(line, endl, iHigh);
              set_turret_position(iLow, iHigh);
              if(!_send_status) send_status_turret();
            }
            break;
          case 's': // 'S'can mode !
            {
              switch(line[2])
              {
              case 'p':
                {
                  // #tsp+90+90$
                  int iLow=0, iHigh=0;
                  int endl = load_int(line, 3, iLow);
                  load_int(line, endl, iHigh);
                  scan_point(iLow, iHigh);
                  if(!_send_status) send_status_turret();
                }
                break;
              case 'l':
                {
                  // #tsl+85+95+1+75$
                  int iLowBegin=0, iLowEnd=0, iLowStep=0, iHigh=0;
                  int endl = load_int(line, 3, iLowBegin);
                  endl = load_int(line, endl, iLowEnd);
                  endl = load_int(line, endl, iLowStep);                
                  load_int(line, endl, iHigh);
                  scan_line(iLowBegin, iLowEnd, iLowStep, iHigh);
                  if(!_send_status) send_status_turret();
                }
                break;
              case 'z':
                {
                  // #tsz+85+95+1+85+95+1$
                  int iLowBegin=0, iLowEnd=0, iLowStep=0, iHighBegin=0, iHighEnd=0, iHighStep;
                  int endl = load_int(line, 3, iLowBegin);
                  endl = load_int(line, endl, iLowEnd);
                  endl = load_int(line, endl, iLowStep);                
                  endl = load_int(line, endl, iHighBegin);
                  endl = load_int(line, endl, iHighEnd);                
                  load_int(line, endl, iHighStep);
                  scan_zone_z(iLowBegin, iLowEnd, iLowStep, iHighBegin, iHighEnd, iHighStep);
                  if(!_send_status) send_status_turret();
                }
                break;
              }
            }
            break;
          }
        }
        break;      
      default:
        if(_send_invalid_cmd)
        {
          String sError = "invalid cmd '" + line + "'";
          send_error(sError);
        }
        break;
      }
    }
  }
}


